#pragma once

// write by lbf_heu 2023/04/15
// 对一条轨迹的仿真

#include "head.hpp"


class path
{
public:
    path(/* args */){

    }
    // 根据时间t算出位置和姿态
    virtual void getPosAndTrans(double t, Eigen::Matrix3d & _Pos, Eigen::Vector3d & _Trans){}
    // 根据时间t计算二维度的trans
    virtual void get2DTrans(double t, Eigen::Vector3d & _Trans){}

    ~path(){
        
    }
};

class path1:public path
{
private:
    /* data */
public:
    path1(/* args */){}
    // 返回一段 x y z 和 pos的运动
    virtual void getPosAndTrans(double t, Eigen::Matrix3d & _Pos, Eigen::Vector3d & _Trans) override {
        double x =  sin(t * 0.2) * 0.95 + 2.5;
        double y =  cos(t * 0.2) * 0.95 + 2.5;
        _Trans << x, y, 0.;// z轴就固定是0吧
        _Pos = AngleAxisd(M_PI/2.5+sin(t)*0.02, Vector3d(0, 0, 1)).matrix();
    }
    virtual void get2DTrans(double t, Eigen::Vector3d & _Trans) override {
        double x =  sin(t * 0.2) * 0.95;
        double y =  cos(t * 0.2) * 0.95;
        _Trans << x, y, 0;
    }

    ~path1(){}
};

